#include "Hmi/cRobotOpenGLView/RoboticOpenGLView.h"
#include <QDebug>
#include <QFile>
#include <QStringList>
#include <QtMath>
#include <QOpenGLBuffer>
#include <QOpenGLShaderProgram>
#include <QOpenGLExtraFunctions>
#include <QMouseEvent>
#include <QWheelEvent>
#include <QCoreApplication>
#include <QDir>

RoboticOpenGLView::RoboticOpenGLView(QWidget *parent) : QOpenGLWidget(parent),
                                  VBO_Base(QOpenGLBuffer::VertexBuffer),
                                  VBO_Joint1(QOpenGLBuffer::VertexBuffer),
                                  xtrans(0),
                                  ytrans(0),
                                  ztrans(0){
    QSurfaceFormat format;
    format.setAlphaBufferSize(24);  //设置alpha缓冲大小
    format.setVersion(3,3);         //设置版本号
    format.setSamples(10);          //设置重采样次数，用于反走样
    this->setFormat(format);

    for(int i = 0; i < 6; i++){ RobotJointPos[i] = 0; }

    QString strPath = QCoreApplication::applicationDirPath(); QDir dir(strPath);
    bool bSuccess = dir.cdUp(); bSuccess = dir.cdUp(); strPath = dir.path();

    vertices_Base =   loadAscllStl(strPath + "/_APP/RobotModel/ZK6/Base.stl",0.001);
    vertices_Joint1 = loadAscllStl(strPath + "/_APP/RobotModel/ZK6/Joint1.stl",0.001);
    vertices_Joint2 = loadAscllStl(strPath + "/_APP/RobotModel/ZK6/Joint2.stl",0.001);
    vertices_Joint3 = loadAscllStl(strPath + "/_APP/RobotModel/ZK6/Joint3.stl",0.001);
    vertices_Joint4 = loadAscllStl(strPath + "/_APP/RobotModel/ZK6/Joint4.stl",0.001);
    vertices_Joint5 = loadAscllStl(strPath + "/_APP/RobotModel/ZK6/Joint5.stl",0.001);
    vertices_Joint6 = loadAscllStl(strPath + "/_APP/RobotModel/ZK6/Joint6.stl",0.001);

}

RoboticOpenGLView::~RoboticOpenGLView(){
    makeCurrent();
}

QVector<float> RoboticOpenGLView::loadAscllStl(QString filename,float ratio){
    QVector<float> vertices_temp;
    qDebug() << "load text file:" << filename;

    QFile file(filename);
    if (!file.open(QIODevice::ReadOnly | QIODevice::Text)){
      qDebug() << "Open stl_file failed." << endl;
    }
    while (!file.atEnd()){
        QString line = file.readLine().trimmed(); // trimmed去除了开头和结尾的空白字符串
        QStringList  words = line.split(' ', QString::SkipEmptyParts);

        if (words[0] == "facet") {
            Normal = {ratio*words[2].toFloat(), ratio*words[3].toFloat(),ratio*words[4].toFloat()};
        }
        else if (words[0] == "vertex") {
            Position = {ratio*words[1].toFloat(), ratio*words[2].toFloat(),ratio*words[3].toFloat()};
            vertices_temp.append(Position);
            vertices_temp.append(Normal);
        }
        else{
          continue;
        }
    }
    qDebug() << "write vertice_temp success!" << filename;
    file.close();
    return vertices_temp;
}

void RoboticOpenGLView::initializeGL(){
    this->initializeOpenGLFunctions();//初始化opengl函数
    shaderprogram.create();//生成着色器程序
    if(!shaderprogram.addShaderFromSourceFile(QOpenGLShader::Vertex,QCoreApplication::applicationDirPath()+ "/stl.vert")){
        qDebug()<<"ERROR:"<<shaderprogram.log();    //如果编译出错,打印报错信息
    }
    if(!shaderprogram.addShaderFromSourceFile(QOpenGLShader::Fragment,QCoreApplication::applicationDirPath() +"/stl.frag")){
        qDebug()<<"ERROR:"<<shaderprogram.log();    //如果编译出错,打印报错信息
    }
    if(!shaderprogram.link()){
         qDebug()<<"ERROR:"<<shaderprogram.log();    //如果链接出错,打印报错信息
    }

    VAO.create();// 创建一个VAO对象，OpenGL会给它（顶点数组缓存对象）分配一个id
    VAO.bind();  //将RC中的当前顶点数组缓存对象Id设置为VAO的id

    VBO_Base.create();
    VBO_Base.bind();

    VBO_Joint1.create();
    VBO_Joint1.bind();

    VBO_Joint2.create();
    VBO_Joint2.bind();

    VBO_Joint3.create();
    VBO_Joint3.bind();

    VBO_Joint4.create();
    VBO_Joint4.bind();

    VBO_Joint5.create();
    VBO_Joint5.bind();

    VBO_Joint6.create();
    VBO_Joint6.bind();

    shaderprogram.setAttributeBuffer("aPos", GL_FLOAT, 0, 3, sizeof(GLfloat) * 6);
    shaderprogram.enableAttributeArray("aPos");
    shaderprogram.setAttributeBuffer("aNormal", GL_FLOAT,sizeof(GLfloat) * 3, 3, sizeof(GLfloat) * 6);
    shaderprogram.enableAttributeArray("aNormal");

    this->glEnable(GL_DEPTH_TEST);

    view.setToIdentity();
    view.translate(0.25,-0.35,0.25);
    view.lookAt(QVector3D(0.0f, 0.0f, 0.0f), QVector3D(0.0f,0.0f,0.0f), QVector3D(0.0f,0.0f,0.0f));
}

void RoboticOpenGLView::resizeGL(int w,int h){
    this->glViewport(0,0,w,h);
    projection.setToIdentity();
    projection.perspective(60.0f, (GLfloat)w/(GLfloat)h, 0.001f, 100.0f);
}

void RoboticOpenGLView::paintGL(){
    this->glClearColor(1.0f, 1.0f, 1.0f, 0.8f);//设置清屏颜色
    this->glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);//清空颜色缓冲区
    shaderprogram.bind();

    QVector3D lightColor(1.0f,1.0f,1.0f);
    QVector3D objectColor(1.0f,0.5f,0.31f);
    QVector3D lightPos(0.0f,0.0f,50.0f);

    shaderprogram.setUniformValue("objectColor",objectColor);
    shaderprogram.setUniformValue("lightColor",lightColor);
    shaderprogram.setUniformValue("lightPos", lightPos);
    shaderprogram.setUniformValue("view", view);
    shaderprogram.setUniformValue("projection", projection);


    model.setToIdentity();
    model.translate(xtrans, ytrans, ztrans);
    model.rotate(rotation);
    shaderprogram.setUniformValue("model", model);
    VBO_Base.allocate(vertices_Base.data(),sizeof(float)*vertices_Base.size());
    this->glDrawArrays(GL_TRIANGLES,0,vertices_Base.capacity()/sizeof(float));

    model.translate(0, 0, 0);
    model.rotate(RobotJointPos[0],0,0,1);
    shaderprogram.setUniformValue("model", model);
    VBO_Joint1.allocate(vertices_Joint1.data(),sizeof(float)*vertices_Joint1.size());
    this->glDrawArrays(GL_TRIANGLES,0,vertices_Joint1.capacity()/sizeof(float));

    model.translate(0.1, 0, 0.48);
    model.rotate(RobotJointPos[1],0,1,0);
    shaderprogram.setUniformValue("model", model);
    VBO_Joint2.allocate(vertices_Joint2.data(),sizeof(float)*vertices_Joint2.size());
    this->glDrawArrays(GL_TRIANGLES,0,vertices_Joint2.capacity()/sizeof(float));

    model.translate(0, 0, 0.58);
    model.rotate(RobotJointPos[2],0,1,0);
    shaderprogram.setUniformValue("model", model);
    VBO_Joint3.allocate(vertices_Joint3.data(),sizeof(float)*vertices_Joint3.size());
    this->glDrawArrays(GL_TRIANGLES,0,vertices_Joint3.capacity()/sizeof(float));

    model.translate(-0.1, 0, 0.177);
    model.rotate(RobotJointPos[3],1,0,0);
    shaderprogram.setUniformValue("model", model);
    VBO_Joint4.allocate(vertices_Joint4.data(),sizeof(float)*vertices_Joint4.size());
    this->glDrawArrays(GL_TRIANGLES,0,vertices_Joint4.capacity()/sizeof(float));

    model.translate(0.77, 0, 0);
    model.rotate(RobotJointPos[4],0,1,0);
    shaderprogram.setUniformValue("model", model);
    VBO_Joint5.allocate(vertices_Joint5.data(),sizeof(float)*vertices_Joint5.size());
    this->glDrawArrays(GL_TRIANGLES,0,vertices_Joint5.capacity()/sizeof(float));

    model.translate(0.1345, 0, 0);
    model.rotate(RobotJointPos[5],1,0,0);
    shaderprogram.setUniformValue("model", model);
    VBO_Joint6.allocate(vertices_Joint6.data(),sizeof(float)*vertices_Joint6.size());
    this->glDrawArrays(GL_TRIANGLES,0,vertices_Joint6.capacity()/sizeof(float));
}


void RoboticOpenGLView::mousePressEvent(QMouseEvent *event){
    mousePos = QVector2D(event->pos());
    event->accept();
}
void RoboticOpenGLView::mouseMoveEvent(QMouseEvent *event){
    if(event->buttons() == Qt::LeftButton){
        QVector2D newPos = (QVector2D)event->pos();
        QVector2D diff = newPos - mousePos;
        qreal angle = (diff.length())/3.6;
        QVector3D rotationAxis = QVector3D(diff.y(), diff.x(), 0.0).normalized();
        rotation = QQuaternion::fromAxisAndAngle(rotationAxis, angle) * rotation;
        mousePos = newPos;
        this->update();
    }
    event->accept();
}
void RoboticOpenGLView::wheelEvent(QWheelEvent *event){
    QPoint numDegrees = event->angleDelta() / 8;
    if (numDegrees.y() > 0) {
        ztrans += 0.25f;
    } else if (numDegrees.y() < 0) {
        ztrans -= 0.25f;
    }
    this->update();
    event->accept();
}

void RoboticOpenGLView::RoboticOpenViewUpdate(){
    this->update();
}

void RoboticOpenGLView::RoboticOpenViewUpdate(double JointPos[6]){
    for(int i = 0; i < 6; i++){
        RobotJointPos[i] = JointPos[i];
    }
    this->update();
}



